//
// Created by sxy on 2021/11/8.
//
#include "map_update/map_update.h"
#include <time.h>
using namespace std;

void readCSGconfig(Cfg &cfg,std::string cfg_parm){
    cfg.readConfigFile(cfg_parm, "slop_smooth", cfg.slop_smooth);
    bool ss = false;
    if (cfg.slop_smooth == "true" || cfg.slop_smooth == "True")
    {
        ss = true;
    }
    else if (cfg.slop_smooth == "false" || cfg.slop_smooth == "False")
    {
        ss = false;
    }
    else{
        if (atoi(cfg.slop_smooth.c_str()) == 0){
            ss = false;
        }
        else
        {
            ss = true;
        }
    }

    cfg.readConfigFile(cfg_parm, "class_threshold", cfg.class_threshold);
    cfg.readConfigFile(cfg_parm, "cloth_resolution", cfg.cloth_resolution);
    cfg.readConfigFile(cfg_parm, "iterations", cfg.iterations);
    cfg.readConfigFile(cfg_parm, "rigidness", cfg.rigidness);
    cfg.readConfigFile(cfg_parm, "time_step", cfg.time_step);
    cfg.readConfigFile(cfg_parm, "terr_pointClouds_filepath", cfg.terr_pointClouds_filepath);

    cfg.csf.params.bSloopSmooth = ss;
    cfg.csf.params.class_threshold = atof(cfg.class_threshold.c_str());
    cfg.csf.params.cloth_resolution = atof(cfg.cloth_resolution.c_str());
    cfg.csf.params.interations = atoi(cfg.iterations.c_str());
    cfg.csf.params.rigidness = atoi(cfg.rigidness.c_str());
    cfg.csf.params.time_step = atof(cfg.time_step.c_str());
}

void deletegroud(Cfg cfg,pcl::PointCloud<pcl::PointXYZ>::Ptr mapCloud,pcl::PointCloud<pcl::PointXYZ>::Ptr withoutground,pcl::PointCloud<pcl::PointXYZ>::Ptr ground){
    cfg.csf.getinputpoint(mapCloud);
    clock_t start,end;
    start=clock();

    std::vector<int> groundIndex,offGroundIndexes;
    cfg.csf.do_filtering(groundIndex,offGroundIndexes,false);

    end=clock();
    double dur = (double)(end - start);
//    printf("Use Time:%f\n", (dur / CLOCKS_PER_SEC));
    cfg.csf.PointWithoutGround(offGroundIndexes,withoutground);
    cfg.csf.PointWithoutGround(groundIndex,ground);

}
void registration(pcl::PointCloud<pcl::PointXYZ>::Ptr source,pcl::PointCloud<pcl::PointXYZ>::Ptr target,Eigen::Matrix4f& transform){
    clock_t starttime,endtime;
    starttime=clock();

    int iterations = 100;
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; // 根据输入数据的尺度设置ICP相关参数
    icp.setInputCloud (source);  //源点云
    icp.setInputTarget (target);  //目标点云
    icp.setTransformationEpsilon (1e-10);   //为终止条件设置最小转换差异
    icp.setMaxCorrespondenceDistance(1); //设置对应点对之间的最大距离（此值对配准结果影响较大）。
    icp.setEuclideanFitnessEpsilon(0.01);  //设置收敛条件是均方误差和小于阈值， 停止迭代；
    icp.setMaximumIterations(iterations); //最大迭代次数，icp是一个迭代的方法，最多迭代这些次（若结合可视化并逐次显示，可将次数设置为1；

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    icp.align (*output_cloud);

    endtime=clock();
    cout << "The run time of registration is: " <<(double)(endtime - starttime) / CLOCKS_PER_SEC << "s" << endl;

    transform=icp.getFinalTransformation();
//    pcl::transformPointCloud (*source, *output_cloud, icp.getFinalTransformation ());
}

void detect_change(pcl::PointCloud<pcl::PointXYZ>::Ptr input_origin,pcl::PointCloud<pcl::PointXYZ>::Ptr input_add,vector<int>& index){
    clock_t starttime,endtime;
    starttime=clock();
    float resolution = 0.2f;
    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);

    // 载入原始点云
    octree.setInputCloud(input_origin->makeShared());
    octree.addPointsFromInputCloud();

    // 切换缓冲区，这会重置八叉树，但是会保留之前八叉树的树结构
    octree.switchBuffers();
    octree.setInputCloud(input_add->makeShared());
    octree.addPointsFromInputCloud();

    // 空间变化检测
//    std::vector<int> detectedPoints;
    int numberOfNewPoints = octree.getPointIndicesFromNewVoxels(index);
    std::cout << numberOfNewPoints << " New Points Detected!" << std::endl;
    endtime=clock();
    cout << "The run time of detect_change is: " <<(double)(endtime - starttime) / CLOCKS_PER_SEC << "s" << endl;
}

void rectangle(double len,vector<double>& leftup,vector<double>& rightup,vector<double>& rightdown,vector<double>& leftdown,double x1,double y1,double x2,double y2){
    vector<double> point11(2);  //加的
    vector<double> point12(2);  //减的
    vector<double> point21(2);
    vector<double> point22(2);
    if(abs(x1-x2)<0.01){
        point11[0]=x1+len;
        point11[1]=y1;

        point11[0]=x1-len;
        point11[1]=y1;

        point21[0]=x2+len;
        point21[1]=y2;

        point22[0]=x2-len;
        point22[1]=y2;
    }
    else if(abs(y1-y2)<0.01){
        point11[0]=x1;
        point11[1]=y1+len;
        point12[0]=x1;
        point12[1]=y1-len;

        point21[0]=x2;
        point21[1]=y2+len;
        point22[0]=x2;
        point22[1]=y2-len;
    }
    else{
        double k1=(y1-y2)/(x1-x2);
        double k2=-1/k1;
        double theta=atan(k2);
        double s=sin(theta);
        double c=cos(theta);

        point11={x1+len*c,y1+len*s};
        point12={x1-len*c,y1-len*s};

        point21={x2+len*c,y2+len*s};
        point22={x2-len*c,y2-len*s};
    }
    leftup[0]=point22[0];
    leftup[1]=point22[1];
    rightup[0]=point21[0];
    rightup[1]=point21[1];
    leftdown[0]=point12[0];
    leftdown[1]=point12[1];
    rightdown[0]=point11[0];
    rightdown[1]=point11[1];
}